#include "StdAfx.h"
#include "Command_Buffer.h"
#include "Command_Commands.h"

#include "CharacterInstance.h"
#include "SkeletonAnim.h"
#include "SkeletonPose.h"

#include "PoseModifier/PoseModifierHelper.h"
void ProcessAnimationDrivenIk(CCharInstance& instance)
{
	DEFINE_PROFILER_FUNCTION();

	if (!instance.m_SkeletonAnim.m_IsAnimPlaying)
		return;

	const CModelSkeleton& modelSkeleton = instance.m_pModel->m_ModelSkeleton;

	Skeleton::CPoseData* pPoseData = instance.m_SkeletonPose.GetPoseDataWriteable();
	QuatT* const __restrict pRelativePose = SPU_PTR_SELECT(&pPoseData->m_pJointsRelative[0], gSkeletonRelativePose);
	QuatT* const __restrict pAbsolutePose = SPU_PTR_SELECT(&pPoseData->m_pJointsAbsolute[0], gSkeletonAbsolutePose);

	uint32 numIKTargets = modelSkeleton.m_ADIKTargets.size();
	for (uint32 i=0; i<numIKTargets; i++)
	{
		uint64 nHandle = modelSkeleton.m_ADIKTargets[i].m_nHandle;

		int32 nIKTargetIdx = modelSkeleton.m_ADIKTargets[i].m_idxTarget;
		assert(nIKTargetIdx < 256); // an index buffer then 256 is invalid
		if (nIKTargetIdx < 0)
			continue; // no Target-Joint

		int32 nIKWeightIdx = modelSkeleton.m_ADIKTargets[i].m_idxWeight;
		assert(nIKWeightIdx < 256); 
		if (nIKWeightIdx < 0)
			continue; // no Weight-Joint

		int32 idxDefinition = modelSkeleton.GetLimbDefinitionIdx(nHandle);
		if (idxDefinition < 0)
			continue; // we didn't find the limb definition and the IK-Solver for this handle

		const IKLimbType& rIKLimbType	= modelSkeleton.m_IKLimbTypes[idxDefinition];

		const QuatT& rAbsIKTarget = pAbsolutePose[nIKTargetIdx];
		f32 fIKTargetBlend = pRelativePose[nIKWeightIdx].t.x;
		fIKTargetBlend = clamp(fIKTargetBlend,0.0f, 1.0f);	
		if (fIKTargetBlend>0.01f)
		{
			uint32 numLinks=rIKLimbType.m_arrJointChain.size();
			int32 nEndEffector = rIKLimbType.m_arrJointChain[numLinks-1].m_idxJoint; //EndEffector
			int32 nParentEndEffector = modelSkeleton.m_arrModelJoints[nEndEffector].m_idxParent; //parent of EndEffector

			const char* _2BIK = "2BIK";
			const char* _3BIK = "3BIK";
			const char* _CCDX = "CCDX";

			Vec3 vLocalTarget; vLocalTarget.SetLerp(pAbsolutePose[nEndEffector].t, rAbsIKTarget.t, fIKTargetBlend);
			if (rIKLimbType.m_nSolver == *(uint32*)_2BIK)
				PoseModifierHelper::IK_Solver2Bones(vLocalTarget, rIKLimbType, pRelativePose, pAbsolutePose);
			if (rIKLimbType.m_nSolver == *(uint32*)_3BIK)
				PoseModifierHelper::IK_Solver3Bones(vLocalTarget, rIKLimbType, pRelativePose, pAbsolutePose);
			if (rIKLimbType.m_nSolver == *(uint32*)_CCDX)
				PoseModifierHelper::IK_SolverCCD(vLocalTarget, rIKLimbType, pRelativePose, pAbsolutePose);

			uint32 numChilds = modelSkeleton.m_IKLimbTypes[idxDefinition].m_arrLimbChildren.size();
			const int16* pJointIdx = &modelSkeleton.m_IKLimbTypes[idxDefinition].m_arrLimbChildren[0];
			for (uint32 u=0; u<numChilds; u++)
			{
				int32 c = pJointIdx[u];
				int32 p = modelSkeleton.m_arrModelJoints[c].m_idxParent;
				pAbsolutePose[c] = pAbsolutePose[p] * pRelativePose[c];
#ifdef _DEBUG
				ANIM_ASSET_ASSERT( pAbsolutePose[c].q.IsUnit() );
				ANIM_ASSET_ASSERT( pAbsolutePose[p].q.IsUnit() );
				ANIM_ASSET_ASSERT( pRelativePose[c].q.IsUnit() );
				ANIM_ASSET_ASSERT( pAbsolutePose[c].q.IsValid() );
				ANIM_ASSET_ASSERT( pAbsolutePose[p].q.IsValid() );
				ANIM_ASSET_ASSERT( pRelativePose[c].q.IsValid() );
#endif			
			}

#ifdef _DEBUG
			ANIM_ASSET_ASSERT( rAbsIKTarget.q.IsUnit() );
			ANIM_ASSET_ASSERT( rAbsIKTarget.q.IsValid() );
			ANIM_ASSET_ASSERT( pAbsolutePose[nEndEffector].q.IsUnit() );
			ANIM_ASSET_ASSERT( pAbsolutePose[nEndEffector].q.IsValid() );
#endif			
			pAbsolutePose[nEndEffector].q.SetNlerp(pAbsolutePose[nEndEffector].q, rAbsIKTarget.q, fIKTargetBlend);

#ifdef _DEBUG
			ANIM_ASSET_ASSERT( pAbsolutePose[nParentEndEffector].q.IsUnit() );
			ANIM_ASSET_ASSERT( pAbsolutePose[nParentEndEffector].q.IsValid() );
			ANIM_ASSET_ASSERT( pAbsolutePose[nEndEffector].q.IsUnit() );
			ANIM_ASSET_ASSERT( pAbsolutePose[nEndEffector].q.IsValid() );
#endif			
			pRelativePose[nEndEffector].q = !pAbsolutePose[nParentEndEffector].q * pAbsolutePose[nEndEffector].q;
		}

#if !defined(__SPU__)
		if (Console::GetInst().ca_DebugADIKTargets)
		{
			const char* pName = modelSkeleton.m_ADIKTargets[i].m_strTarget;

			float fColor[4] = {0,1,0,1};
			g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fColor, false,"LHand_IKTarget: name: %s  rot: (%f %f %f %f)  pos: (%f %f %f) blend: %f",pName, rAbsIKTarget.q.w,rAbsIKTarget.q.v.x,rAbsIKTarget.q.v.y,rAbsIKTarget.q.v.z,  rAbsIKTarget.t.x,rAbsIKTarget.t.y,rAbsIKTarget.t.z, pRelativePose[nIKWeightIdx].t.x);	
			g_YLine+=16.0f;

			static Ang3 angle(0,0,0); 	angle+=Ang3(0.01f,+0.02f,+0.03f);
			AABB aabb = AABB(Vec3(-0.015f,-0.015f,-0.015f),Vec3(+0.015f,+0.015f,+0.015f));

			Matrix33 m33=Matrix33::CreateRotationXYZ(angle);
			OBB obb=OBB::CreateOBBfromAABB( m33,aabb );
			g_pAuxGeom->DrawOBB(obb,rAbsIKTarget.t,1,RGBA8(0xff,0x00,0x1f,0xff),eBBD_Extremes_Color_Encoded);
		}
#endif
	}
}

using namespace Command;

/*
CState
*/

bool CState::Initialize(CCharInstance* pInstance, const QuatT& locationPhysics, const QuatTS& locationAnimation)
{
	m_pInstance = pInstance;
	m_pModel = pInstance->m_pModel;

	m_locationPhysics = locationPhysics;
	m_locationAnimation = locationAnimation;

	m_pPoseData = pInstance->m_SkeletonPose.GetPoseDataWriteable();

	m_jointCount = pInstance->m_SkeletonPose.GetJointCount();

	m_lod = m_pInstance->GetAnimationLOD();

	m_timeDelta = m_pInstance->m_fDeltaTime;

	m_pJointMask = NULL;
	m_jointMaskCount = 0;

	m_bFootPlants = pInstance->m_SkeletonPose.m_FootPlants;

	return true;
}

/*
CBuffer
*/

SPU_NO_INLINE bool CBuffer::Initialize(CCharInstance* pInstance, const QuatT& locationPhysics, const QuatTS& locationAnimation)
{
	m_pCommands = m_pBuffer;
	m_commandCount = 0;

	m_state.Initialize(pInstance, locationPhysics, locationAnimation);

	m_pInstance = pInstance;
	return true;
}

void CBuffer::SetPoseData(Skeleton::CPoseData& poseData)
{
	m_state.m_pPoseData = &poseData;
}

//on SPU we cannot efficiently handle switch statements






	#define SWITCH(c) switch(c)
	#define BREAK break
	#define CASE(command,c) case (c):
	#define CASE_0 CASE


SPU_NO_INLINE void CBuffer::Execute()
{
	DEFINE_PROFILER_FUNCTION();




















	QuatT pJointsTemp[MAX_JOINT_AMOUNT];

	QuatT* const __restrict pJointsRelative = SPU_PTR_SELECT(&m_state.m_pPoseData->m_pJointsRelative[0], gSkeletonRelativePose);
	Status4* const __restrict pJointsStatus = SPU_PTR_SELECT(&m_state.m_pPoseData->m_pJointsStatus[0], gSkeletonControllerInfo);

	::memset(pJointsStatus, 0, m_state.m_jointCount * sizeof(Status4));

	m_state.m_pJointMask = NULL;
	m_state.m_jointMaskCount = 0;

	void* CBTemp[TargetBuffer+3];
	CBTemp[TmpBuffer+0]	= pJointsTemp;
	CBTemp[TmpBuffer+1]	= pJointsStatus;

	CBTemp[TargetBuffer+0] = pJointsRelative;
	CBTemp[TargetBuffer+1] = pJointsStatus;

	uint8* pCommands = m_pBuffer;
	uint32 commandOffset = 0;
	for (uint32 i=0; i<m_commandCount; ++i)
	{
		uint8& command = pCommands[commandOffset];
		SWITCH(command)
		{
		CASE_0(command,ClearFull::ID)
			{
				((ClearFull&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(ClearFull);
				BREAK;
			}

		CASE(command,ClearSingle::ID)
			{
				((ClearSingle&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(ClearSingle);
				BREAK;
			}

		CASE(command,SampleAddAnimFull::ID)
			{
				((SampleAddAnimFull&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(SampleAddAnimFull);
				BREAK;
			}

		CASE(command,CopyAddAnimFull::ID)
			{
				((CopyAddAnimFull&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(CopyAddAnimFull);
				BREAK;
			}

		CASE(command,NormalizeFull::ID)
			{
				((NormalizeFull&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(NormalizeFull);
				BREAK;
			}

		CASE(command,ScaleUniformFull::ID)
			{
				((ScaleUniformFull&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(ScaleUniformFull);
				BREAK;
			}
		CASE(command,PoseModifier::ID)
			{
				((PoseModifier&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(PoseModifier);
				BREAK;
			}

		CASE(command,SampleAnimPart::ID)
			{
				((SampleAnimPart&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(SampleAnimPart);
				BREAK;
			}

		CASE(command,SampleFeatherAnimPart::ID)
			{
				((SampleFeatherAnimPart&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(SampleFeatherAnimPart);
				BREAK;
			}

		CASE(command,JointMask::ID)
			{
				if (Console::GetInst().ca_UseJointMasking)
				{
					m_state.m_pJointMask = ((JointMask&)command).m_pMask;
					m_state.m_jointMaskCount = ((JointMask&)command).m_count;
				}
				commandOffset += sizeof(JointMask);
				BREAK;
			}

		CASE(command,SampleAddPoseFull::ID)
			{
				((SampleAddPoseFull&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(SampleAddPoseFull);
				BREAK;
			}

		CASE(command,SamplePosePart::ID)
			{
				((SamplePosePart&)command).Execute(m_state, CBTemp);
				commandOffset += sizeof(SamplePosePart);
				BREAK;
			}
#ifndef __SPU__
	#ifdef _DEBUG
		case VerifyFull::ID:
			((VerifyFull&)command).Execute(m_state, CBTemp);
			commandOffset += sizeof(VerifyFull);
			BREAK;
	#endif
		default:
			CryFatalError("CryAnimation: Command-Buffer Invalid: Command %d  Model: %s",command,m_state.m_pModel->GetModelFilePath());
			BREAK;
#endif
		}
	}

#ifndef _RELEASE	
	m_state.m_pPoseData->ValidateRelPose(&m_state.m_pModel->m_ModelSkeleton.m_arrModelJoints[0]);
#endif
	m_state.m_pPoseData->ComputeAbsolutePose(	&m_state.m_pModel->m_ModelSkeleton.m_arrModelJoints[0],	m_state.m_pModel->m_ObjectType == CHR);
#ifndef _RELEASE	
	m_state.m_pPoseData->ValidateAbsPose(&m_state.m_pModel->m_ModelSkeleton.m_arrModelJoints[0]);
#endif

	SAnimationPoseModiferParams params;
	params.pCharacterInstance = m_state.m_pInstance;
	params.timeDelta = m_state.m_timeDelta;
	params.locationNextPhysics = m_state.m_locationPhysics;
	params.locationNextAnimation = m_state.m_locationAnimation;
	params.pPoseRelative = SPU_PTR_SELECT(&m_state.m_pPoseData->m_pJointsRelative[0], gSkeletonRelativePose);
	params.pPoseAbsolute = SPU_PTR_SELECT(&m_state.m_pPoseData->m_pJointsAbsolute[0], gSkeletonAbsolutePose);
	params.jointCount = m_state.m_jointCount;

	DynArray<IAnimationPoseModifierPtr>& poseModifiers = m_pInstance->m_SkeletonAnim.m_poseModifiers;
	const uint32 poseModifierCount = uint32(poseModifiers.size());
	for (uint32 i=0; i<poseModifierCount; ++i)
		poseModifiers[i].get()->Execute(params);

	ProcessAnimationDrivenIk(*m_pInstance);

	m_pInstance->m_SkeletonPose.SPU_SynchronizeWithPhysics(
		*m_state.m_pPoseData, m_state.m_locationPhysics, m_state.m_locationAnimation);

#if !defined(__SPU__)
	if (Console::GetInst().ca_DebugCommandBuffer)
		DebugDraw();
#endif

	// SPU post block, used to transfer finished data back to ppu










}

#undef SWITCH
#undef BREAK
#undef CASE
#undef CASE_0


void CBuffer::DebugDraw()
{
	float charsize =1.4f;
	uint32 yscan =  14;
	float fColor2[4] = {1,0,1,1};
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor2, false,
		"m_CommandBufferCounter %d  Commands: %d", GetLengthUsed(), m_commandCount);
	g_YLine+=10;

	IAnimationSet* pAnimationSet = m_state.m_pModel->GetAnimationSet();

	uint8* pCommands = m_pBuffer;
	uint32 commandOffset = 0;
	for (uint32 c=0; c<m_commandCount; c++)
	{
		int32 anim_command = pCommands[commandOffset];
		switch (anim_command)
		{
		case ClearFull::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "BaseAnim_CLEAR_BUFFER"); g_YLine+=yscan;
			commandOffset += sizeof(ClearFull);
			break;

		case ClearSingle::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "BaseAnim_CLEAR_ROOT"); g_YLine+=yscan;
			commandOffset += sizeof(ClearSingle);
			break;

		case SampleAddAnimFull::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "BaseAnim_FETCH_ADD: %f %s", ((SampleAddAnimFull*)&pCommands[commandOffset])->m_fWeight, pAnimationSet->GetNameByAnimID( ((SampleAddAnimFull*)&pCommands[commandOffset])->m_nEAnimID ) ); g_YLine+=yscan;
			commandOffset += sizeof(SampleAddAnimFull);
			break;

		case CopyAddAnimFull::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "BaseAnim_COPY_ADD"); g_YLine+=yscan;
			commandOffset += sizeof(CopyAddAnimFull);
			break;

		case NormalizeFull::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "BaseAnim_NORMALIZE"); g_YLine+=yscan;
			commandOffset += sizeof(NormalizeFull);
			break;

		case ScaleUniformFull::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "BaseAnim_USCALE"); g_YLine+=yscan;
			commandOffset += sizeof(ScaleUniformFull);
			break;

#ifdef _DEBUG
		case VerifyFull::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "BaseAnim_VERIFY"); g_YLine+=yscan;
			commandOffset += sizeof(VerifyFull);
			break;
#endif

		case PoseModifier::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "Proc_POSEMOD: %s", ((PoseModifier*)&pCommands[commandOffset])->m_pPoseModifier->GetFactory()->GetClassName() ); g_YLine+=yscan;
			commandOffset += sizeof(PoseModifier);
			break;

		case SampleAnimPart::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "PartAnim_FETCH_ADD: %f %s", ((SampleAnimPart*)&pCommands[commandOffset])->m_fWeight, pAnimationSet->GetNameByAnimID( ((SampleAnimPart*)&pCommands[commandOffset])->m_nEAnimID ) ); g_YLine+=yscan;
			commandOffset += sizeof(SampleAnimPart);
			break;

		case SampleFeatherAnimPart::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "PartAnim_FEATHERFETCH"); g_YLine+=yscan;
			commandOffset += sizeof(SampleFeatherAnimPart);
			break;

		case JointMask::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "State_JOINTMASK"); g_YLine+=yscan;
			commandOffset += sizeof(JointMask);
			break;

		case SampleAddPoseFull::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "BasePose_FETCH_ADD: %f %s", ((SampleAddPoseFull*)&pCommands[commandOffset])->m_fWeight, pAnimationSet->GetNameByAnimID( ((SampleAddPoseFull*)&pCommands[commandOffset])->m_nEAnimID ) ); g_YLine+=yscan;
			commandOffset += sizeof(SampleAddPoseFull);
			break;

		case SamplePosePart::ID:
			g_pIRenderer->Draw2dLabel(1,g_YLine, charsize, fColor2, false, "PartPose_FETCH_ADD: %f %s", ((SamplePosePart*)&pCommands[commandOffset])->m_fWeight, pAnimationSet->GetNameByAnimID( ((SamplePosePart*)&pCommands[commandOffset])->m_nEAnimID ) ); g_YLine+=yscan;
			commandOffset += sizeof(SamplePosePart);
			break;

		default:
			CryFatalError("CryAnimation: Command-Buffer Invalid: Command %d  Model: %s", anim_command, m_state.m_pModel->GetModelFilePath());
			break;
		}
	}
}
